#ifndef _MATRIX_H_
#define _MATRIX_H_

#include <assert.h>
#include <string>

#include "misc.h"
#include "Vector.h"

class Matrix4f;

class Matrix3f{

	friend class Matrix4f;

private:
	real _m[3][3];

public:
	Matrix3f()
	{
		_m[0][0] = 0.0f;
		_m[0][1] = 0.0f;
		_m[0][2] = 0.0f;
		_m[1][0] = 0.0f;
		_m[1][1] = 0.0f;
		_m[1][2] = 0.0f;
		_m[2][0] = 0.0f;
		_m[2][1] = 0.0f;
		_m[2][2] = 0.0f;
	}		

	inline void Set(int x, int y, real v)
	{
		_m[x][y] = v;
	}

	inline real Get(int x, int y) const
	{
		return _m[x][y];
	}	

	inline real GetDet() const
	{
		return _m[0][0] * (_m[1][1] * _m[2][2] - _m[1][2] * _m[2][1])
			-	 _m[1][0] * (_m[0][1] * _m[2][2] - _m[0][2] * _m[2][1])
			+	 _m[2][0] * (_m[0][1] * _m[1][2] - _m[0][2] * _m[1][1]);
	}

	inline Matrix3f GetInverted() const
	{
		Matrix3f r;

		real det = GetDet();

		r._m[0][0] =  (_m[1][1] * _m[2][2] - _m[2][1] * _m[1][2])  / det;
		r._m[1][0] = -(_m[1][0] * _m[2][2] - _m[1][2] * _m[2][0])  / det;
		r._m[2][0] =  (_m[1][0] * _m[2][1] - _m[1][1] * _m[2][0])  / det;

		r._m[0][1] = -(_m[0][1] * _m[2][2] - _m[2][1] * _m[0][2])  / det;
		r._m[1][1] =  (_m[0][0] * _m[2][2] - _m[0][2] * _m[2][0])  / det;
		r._m[2][1] = -(_m[0][0] * _m[2][1] - _m[0][1] * _m[2][0])  / det;

		r._m[0][2] =  (_m[0][1] * _m[1][2] - _m[0][2] * _m[1][1])  / det;
		r._m[1][2] = -(_m[0][0] * _m[1][2] - _m[0][2] * _m[1][0])  / det;
		r._m[2][2] =  (_m[0][0] * _m[1][1] - _m[1][0] * _m[0][1])  / det;

		return r;
	}

	std::string ToString() const
	{
		std::string s = "[ ";
		for(int y = 0; y<3; ++y)
		{
			for(int x = 0; x<3; ++x)
			{
				s = s + To_String<real>( _m[x][y] ) + "  ";
			}
			if(y < 2)
				s = s + "\n  ";
			else
				s = s + " ]";
		}

		return s;
	}

};

class Matrix4f{

	friend class Vector4f;

private:

	real _m[4][4];

public:

	inline Matrix4f()
	{
		_m[0][0] = 0.0f;
		_m[0][1] = 0.0f;
		_m[0][2] = 0.0f;
		_m[0][3] = 0.0f;

		_m[1][0] = 0.0f;
		_m[1][1] = 0.0f;
		_m[1][2] = 0.0f;
		_m[1][3] = 0.0f;

		_m[2][0] = 0.0f;
		_m[2][1] = 0.0f;
		_m[2][2] = 0.0f;
		_m[2][3] = 0.0f;

		_m[3][0] = 0.0f;
		_m[3][1] = 0.0f;
		_m[3][2] = 0.0f;
		_m[3][3] = 0.0f;
	}

	inline void SetIdentity()
	{
		_m[0][0] = 1.0f;
		_m[0][1] = 0.0f;
		_m[0][2] = 0.0f;
		_m[0][3] = 0.0f;

		_m[1][0] = 0.0f;
		_m[1][1] = 1.0f;
		_m[1][2] = 0.0f;
		_m[1][3] = 0.0f;

		_m[2][0] = 0.0f;
		_m[2][1] = 0.0f;
		_m[2][2] = 1.0f;
		_m[2][3] = 0.0f;

		_m[3][0] = 0.0f;
		_m[3][1] = 0.0f;
		_m[3][2] = 0.0f;
		_m[3][3] = 1.0f;
	}

	inline Vector4f GetPos() const
	{
		Vector4f v(	_m[3][0], _m[3][1], _m[3][2], _m[3][3] );
		return v;
	}

	inline Vector4f GetCol(int i) const
	{
		Vector4f v(	_m[i][0], _m[i][1], _m[i][2], _m[i][3] );
		return v;
	}

	inline void SetPos( Vector4f & pos )
	{
		_m[3][0] = pos._x;
		_m[3][1] = pos._y;
		_m[3][2] = pos._z;
		_m[3][3] = pos._w;
	}

	inline void SetPos( real x, real y, real z, real w)
	{
		_m[3][0] = x;
		_m[3][1] = y;
		_m[3][2] = z;
		_m[3][3] = w;
	}

	inline void Set(int col, int row, real value)
	{
		_m[col][row] = value;
	}

	inline real Get(int col, int row) const
	{
		return _m[col][row];
	}

	inline real GetX() const
	{
		return _m[3][0];
	}

	inline real GetY() const
	{
		return _m[3][1];
	}

	inline real GetZ() const
	{
		return _m[3][2];
	}

	inline void SetX(real x)
	{
		_m[3][0] = x;
	}

	inline void SetY(real y)
	{
		_m[3][1] = y;
	}

	inline void SetZ(real z)
	{
		_m[3][2] = z;
	}

	inline Matrix4f Add(const Matrix4f & mat) const
	{
		Matrix4f r;
		r._m[0][0] = _m[0][0] + mat._m[0][0];
		r._m[0][1] = _m[0][1] + mat._m[0][1];
		r._m[0][2] = _m[0][2] + mat._m[0][2];
		r._m[0][3] = _m[0][3] + mat._m[0][3];
		r._m[1][0] = _m[1][0] + mat._m[1][0];
		r._m[1][1] = _m[1][1] + mat._m[1][1];
		r._m[1][2] = _m[1][2] + mat._m[1][2];
		r._m[1][3] = _m[1][3] + mat._m[1][3];
		r._m[2][0] = _m[2][0] + mat._m[2][0];
		r._m[2][1] = _m[2][1] + mat._m[2][1];
		r._m[2][2] = _m[2][2] + mat._m[2][2];
		r._m[2][3] = _m[2][3] + mat._m[2][3];
		r._m[3][0] = _m[3][0] + mat._m[3][0];
		r._m[3][1] = _m[3][1] + mat._m[3][1];
		r._m[2][2] = _m[2][2] + mat._m[2][2];
		r._m[3][3] = _m[3][3] + mat._m[3][3];

		return r;		
	}

	inline Matrix4f Sub(const Matrix4f & mat) const
	{
		Matrix4f r;
		r._m[0][0] = _m[0][0] - mat._m[0][0];
		r._m[0][1] = _m[0][1] - mat._m[0][1];
		r._m[0][2] = _m[0][2] - mat._m[0][2];
		r._m[0][3] = _m[0][3] - mat._m[0][3];
		r._m[1][0] = _m[1][0] - mat._m[1][0];
		r._m[1][1] = _m[1][1] - mat._m[1][1];
		r._m[1][2] = _m[1][2] - mat._m[1][2];
		r._m[1][3] = _m[1][3] - mat._m[1][3];
		r._m[2][0] = _m[2][0] - mat._m[2][0];
		r._m[2][1] = _m[2][1] - mat._m[2][1];
		r._m[2][2] = _m[2][2] - mat._m[2][2];
		r._m[2][3] = _m[2][3] - mat._m[2][3];
		r._m[3][0] = _m[3][0] - mat._m[3][0];
		r._m[3][1] = _m[3][1] - mat._m[3][1];
		r._m[2][2] = _m[2][2] - mat._m[2][2];
		r._m[3][3] = _m[3][3] - mat._m[3][3];

		return r;		
	}

	inline Matrix4f Mul(const Matrix4f & mat) const
	{
		Matrix4f r;
		r._m[0][0] = _m[0][0] * mat._m[0][0] + _m[1][0] * mat._m[0][1] + _m[2][0] * mat._m[0][2] + _m[3][0] * mat._m[0][3];
		r._m[0][1] = _m[0][1] * mat._m[0][0] + _m[1][1] * mat._m[0][1] + _m[2][1] * mat._m[0][2] + _m[3][1] * mat._m[0][3];
		r._m[0][2] = _m[0][2] * mat._m[0][0] + _m[1][2] * mat._m[0][1] + _m[2][2] * mat._m[0][2] + _m[3][2] * mat._m[0][3];
		r._m[0][3] = _m[0][3] * mat._m[0][0] + _m[1][3] * mat._m[0][1] + _m[2][3] * mat._m[0][2] + _m[3][3] * mat._m[0][3];
		r._m[1][0] = _m[0][0] * mat._m[1][0] + _m[1][0] * mat._m[1][1] + _m[2][0] * mat._m[1][2] + _m[3][0] * mat._m[1][3];
		r._m[1][1] = _m[0][1] * mat._m[1][0] + _m[1][1] * mat._m[1][1] + _m[2][1] * mat._m[1][2] + _m[3][1] * mat._m[1][3];
		r._m[1][2] = _m[0][2] * mat._m[1][0] + _m[1][2] * mat._m[1][1] + _m[2][2] * mat._m[1][2] + _m[3][2] * mat._m[1][3];
		r._m[1][3] = _m[0][3] * mat._m[1][0] + _m[1][3] * mat._m[1][1] + _m[2][3] * mat._m[1][2] + _m[3][3] * mat._m[1][3];
		r._m[2][0] = _m[0][0] * mat._m[2][0] + _m[1][0] * mat._m[2][1] + _m[2][0] * mat._m[2][2] + _m[3][0] * mat._m[2][3];
		r._m[2][1] = _m[0][1] * mat._m[2][0] + _m[1][1] * mat._m[2][1] + _m[2][1] * mat._m[2][2] + _m[3][1] * mat._m[2][3];
		r._m[2][2] = _m[0][2] * mat._m[2][0] + _m[1][2] * mat._m[2][1] + _m[2][2] * mat._m[2][2] + _m[3][2] * mat._m[2][3];
		r._m[2][3] = _m[0][3] * mat._m[2][0] + _m[1][3] * mat._m[2][1] + _m[2][3] * mat._m[2][2] + _m[3][3] * mat._m[2][3];
		r._m[3][0] = _m[0][0] * mat._m[3][0] + _m[1][0] * mat._m[3][1] + _m[2][0] * mat._m[3][2] + _m[3][0] * mat._m[3][3];
		r._m[3][1] = _m[0][1] * mat._m[3][0] + _m[1][1] * mat._m[3][1] + _m[2][1] * mat._m[3][2] + _m[3][1] * mat._m[3][3];
		r._m[3][2] = _m[0][2] * mat._m[3][0] + _m[1][2] * mat._m[3][1] + _m[2][2] * mat._m[3][2] + _m[3][2] * mat._m[3][3];
		r._m[3][3] = _m[0][3] * mat._m[3][0] + _m[1][3] * mat._m[3][1] + _m[2][3] * mat._m[3][2] + _m[3][3] * mat._m[3][3];

		return r;		
	}

	inline Vector4f Mul(const Vector4f & v) const
	{
		Vector4f r;

		r._x = v._x * _m[0][0] + v._y * _m[1][0] + v._z * _m[2][0] + v._w * _m[3][0];
		r._y = v._x * _m[0][1] + v._y * _m[1][1] + v._z * _m[2][1] + v._w * _m[3][1];
		r._z = v._x * _m[0][2] + v._y * _m[1][2] + v._z * _m[2][2] + v._w * _m[3][2];
		r._w = v._x * _m[0][3] + v._y * _m[1][3] + v._z * _m[2][3] + v._w * _m[3][3];	

		return r;		
	}

	inline Matrix4f Mul(const real c) const
	{
		Matrix4f r;
		r._m[0][0] = _m[0][0] * c;
		r._m[0][1] = _m[0][1] * c;
		r._m[0][2] = _m[0][2] * c;
		r._m[0][3] = _m[0][3] * c;
		r._m[1][0] = _m[1][0] * c;
		r._m[1][1] = _m[1][1] * c;
		r._m[1][2] = _m[1][2] * c;
		r._m[1][3] = _m[1][3] * c;
		r._m[2][0] = _m[2][0] * c;
		r._m[2][1] = _m[2][1] * c;
		r._m[2][2] = _m[2][2] * c;
		r._m[2][3] = _m[2][3] * c;
		r._m[3][0] = _m[3][0] * c;
		r._m[3][1] = _m[3][1] * c;
		r._m[2][2] = _m[2][2] * c;
		r._m[3][3] = _m[3][3] * c;

		return r;		
	}

	inline Matrix3f GetSubMat(int col, int line) const
	{
		Matrix3f sub_mat;

		int rx = 0;
		for(int x=0; x<4; ++x)
		{
			if(x != col)
			{
				int ry = 0;
				for(int y=0; y<4; ++y)
				{
					if(y != line)
					{
						sub_mat._m[rx][ry] = _m[x][y];
						ry++;
					}				
				}
				rx++;
			}
		}		

		return sub_mat;	
	}

	inline real GetDet() const
	{
		real det3;
		real det4 = 0;

		real sign = 1;
		for(int x = 0; x < 4; ++x)
		{
			Matrix3f sub_mat = GetSubMat(x, 0);
			det3 = sub_mat.GetDet();
			det4 += _m[x][0] * det3 * sign;
			sign *= -1;			
		}

		return det4;		
	}

	inline Matrix4f GetInverted() const
	{
		Matrix4f r;

		real det4 = GetDet();

		for(int x = 0; x < 4; ++x)
		{
			for(int y = 0; y < 4; ++y)
			{
				int sign = 1 - ((x + y) % 2) * 2;
				Matrix3f mat3 = GetSubMat(y, x);
				real det3 = mat3.GetDet();
				r._m[x][y] = det3 / det4 * (real) sign;
			}
		}

		return r;
	}

	std::string ToString() const
	{
		std::string s = "[ ";
		for(int y = 0; y<4; ++y)
		{
			for(int x = 0; x<4; ++x)
			{
				s = s + To_String<real>( _m[x][y] ) + "  ";
			}
			if(y < 3)
				s = s + "\n  ";
			else
				s = s + " ]";
		}

		return s;
	}

	inline void SetRotationX(real alpha)
	{
		SetIdentity();
		Set(1,1,cos(alpha));
		Set(2,1,-sin(alpha));
		Set(1,2,sin(alpha));
		Set(2,2,cos(alpha));		
	}

	inline void SetRotationY(real alpha)
	{
		SetIdentity();
		Set(0,0,cos(alpha));
		Set(2,0,-sin(alpha));
		Set(0,2,sin(alpha));
		Set(2,2,cos(alpha));		
	}

	inline void SetRotationZ(real alpha)
	{
		SetIdentity();
		Set(0,0,cos(alpha));
		Set(1,0,-sin(alpha));
		Set(0,1,sin(alpha));
		Set(1,1,cos(alpha));		
	}

};

inline Matrix4f operator+ (const Matrix4f & l, const Matrix4f & r) 	
{
	return l.Add(r);
}

inline Matrix4f operator- (const Matrix4f & l, const Matrix4f & r) 	
{
	return l.Sub(r);
}

inline Matrix4f operator* (const Matrix4f & l, const Matrix4f & r) 	
{
	return l.Mul(r);
}

inline Vector4f operator* (const Matrix4f & l, const Vector4f & r) 	
{
	return l.Mul(r);
}

inline Vector4f operator* (const Vector4f & l, const Matrix4f & r) 	
{
	return r.Mul(l);
}

inline Matrix4f operator* (const Matrix4f & l, const real r) 	
{
	return l.Mul(r);
}

inline Matrix4f operator* (const real l, const Matrix4f & r) 	
{
	return r.Mul(l);
}


#endif
